#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <nav_msgs/Odometry.h>
#include <laser_geometry/laser_geometry.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/registration/icp.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <sensor_msgs/point_cloud_conversion.h>  // 添加这个头文件

class LidarToOdom {
public:
    LidarToOdom(ros::NodeHandle& nh) : nh_(nh), lp_(), tf_br_(), tf_listener_(ros::Duration(5)) {
        laser_sub_ = nh_.subscribe<sensor_msgs::LaserScan>("/scan", 1, &LidarToOdom::laserCallback, this);
        odom_pub_ = nh_.advertise<nav_msgs::Odometry>("lidar_odom", 50);
    }

private:
    ros::NodeHandle& nh_;
    laser_geometry::LaserProjection lp_;
    ros::Subscriber laser_sub_;
    ros::Publisher odom_pub_;
    tf::TransformBroadcaster tf_br_;
    tf::TransformListener tf_listener_;
    pcl::PointCloud<pcl::PointXYZ>::Ptr prev_cloud_ = nullptr;  // 初始化指针成员变量
    bool first_scan_ = true;

    void laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan) {
        auto start_time = std::chrono::high_resolution_clock::now(); // 开始时间

        sensor_msgs::PointCloud cloud_msg;
        try {
            lp_.projectLaser(*scan, cloud_msg);  // 指定目标坐标系
            cloud_msg.header.frame_id = "base_footprint";
        } catch (tf::TransformException& ex) {
            ROS_ERROR("Transform failure: %s", ex.what());
            return;
        }

        // 将 sensor_msgs::PointCloud 转换为 sensor_msgs::PointCloud2
        sensor_msgs::PointCloud2 cloud2_msg;
        sensor_msgs::convertPointCloudToPointCloud2(cloud_msg, cloud2_msg);

        pcl::PointCloud<pcl::PointXYZ>::Ptr current_cloud_(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::fromROSMsg(cloud2_msg, *current_cloud_);

        if (first_scan_) {
            prev_cloud_ = current_cloud_;  // 更新前一帧点云
            first_scan_ = false;
            return;
        }

        // 使用 ICP 算法进行点云配准
        pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
        icp.setInputSource(current_cloud_);
        icp.setInputTarget(prev_cloud_);
        pcl::PointCloud<pcl::PointXYZ> final;
        icp.align(final);

        // 获取变换矩阵
        Eigen::Matrix4f transformation = icp.getFinalTransformation();
        Eigen::Vector3f translation = transformation.block<3, 1>(0, 3);
        Eigen::Matrix3f rotation = transformation.block<3, 3>(0, 0);

        // 计算位姿变化
        static double x = 0.0, y = 0.0, theta = 0.0;
        x += translation(0);
        y += translation(1);
        theta += atan2(rotation(1, 0), rotation(0, 0));

        nav_msgs::Odometry odom;
        odom.header.stamp = scan->header.stamp;
        odom.header.frame_id = "odom";
        odom.child_frame_id = "base_footprint";

        // 设置位置
        odom.pose.pose.position.x = x;
        odom.pose.pose.position.y = y;
        odom.pose.pose.position.z = 0.0;
        odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(theta);

        // 设置速度
        odom.twist.twist.linear.x = translation(0);
        odom.twist.twist.angular.z = atan2(rotation(1, 0), rotation(0, 0));

        // 发布里程计消息
        odom_pub_.publish(odom);

        // 更新前一帧点云
        prev_cloud_ = current_cloud_;

        // ROS_INFO("x = %.2f  y= %.2f" x,y);
    }
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "lidar_to_odom");
    ros::NodeHandle nh;

    LidarToOdom lidar_to_odom(nh);

    ros::spin();

    return 0;
}
